﻿/*!
 * @file mixtureofgaussian.cpp
 * @author 橋本敦史
 * @date Last Change:2010/Jun/18.
 */
#include <BackgroundSubtractor/dynamicmixtureofgaussian.h>

using namespace std;

namespace mmpl{
	namespace image{
		/*!
		 * @brief P(サンプル|モデル)を計算する
		 * @param sample 観測された値
		 * @param model 仮定する混合ガウス分布
		 * @param probs modelの各ガウス分布におけるP(サンプル|単分布)
		 * */
		void calcProb(MmplVector& sample,MixtureOfSphericalGaussian& model,MmplVector* probs){
			int dims = sample.size();
			probs->clear();
			probs->resize(static_cast<int>(model.distributions.size()),0.0);

			for(int k=0;k<static_cast<int>(model.distributions.size());k++){
				if(model.distributions.at(k).sigma==0){
					(*probs)[k] = 0;
					continue;
				}

				/* 各分布に対して
				 * p(x) = {1/sqrt(2pi*sigma)} * exp{(x-u)^2/(-2*sigma)}
				 * を計算する
				 * */
				for(int d=0;d<dims;d++){
					/* value = (x-u)^2 */
					(*probs)[k] += pow(sample.at(d)-model.distributions.at(k).means.at(d),2);
				}

				/* value =/ -2 sigma */
				(*probs)[k] /= -2 * model.distributions.at(k).sigma;

				/* value = exp(value) / sqrt(2pi*sigma) */
				(*probs)[k] = pow(2.7182818,(*probs)[k])/ (2.50662827 * sqrt(model.distributions.at(k).sigma));
				// 2.7182818 = e (自然対数の底)
				// 2.50662827 = sqrt(2pi)

			}
		}

		/*!
		 * @brief 正規分布のデータをCSVとして出力する
		 * @param dist 出力する正規分布
		 * @param out 出力先のストリーム
		 * */
		void print(const SphericalGaussian& dist, std::ostream& out){
			out << dist.sigma << endl;
			out << dist.means.print() << endl;
		}

		/*!
		 * @brief 混合正規分布のデータをCSVとして出力する
		 * @param mog 出力する混合正規分布
		 * @param out 出力先のストリーム
		 * */
		void print(const MixtureOfSphericalGaussian& mog, std::ostream& out){
			out << mog.weights.print() << endl;
			for(unsigned int i=0;i<mog.weights.size();i++){
				print(mog.distributions[i],out);
			}
		}

		/*!
		 * @brief 正規分布のデータをCSVから読み出す
		 * @param dist 入力先の正規分布
		 * @param out 入力元のストリーム
		 * */
		void readFromCsv(SphericalGaussian* dist, CSVReader& csvr){
			vector<string> col = csvr.getCol();
			if(col.size()!=1){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: Unexpected input: ";
				for(size_t i=0;i<col.size();i++){
					cerr << col[i] << ",";
				}
				cerr << endl;
			}
			dist->sigma = atof(col[0].c_str());

			col = csvr.getCol();
			if(col.empty()){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: No means has been input." << endl;
				return;
			}
			for(size_t i=0;i<col.size();i++){
				dist->means.push_back(atof(col[i].c_str()));
			}

		}

		/*!
		 * @brief 混合正規分布のデータをCSVから読み出す
		 * @param mog 入力先の混合正規分布
		 * @param out 入力元のストリーム
		 * */
		void readFromCsv(MixtureOfSphericalGaussian* mog, CSVReader& csvr){
			vector<string> col = csvr.getCol();
			for(size_t i=0;i<col.size();i++){
				mog->weights.push_back(atof(col[i].c_str()));
			}
			for(unsigned int i=0;i<mog->weights.size();i++){
				SphericalGaussian newDist;
				readFromCsv(&newDist,csvr);
				mog->distributions.push_back(newDist);
			}

		}

		/*!
		 * @brief デフォルトコンストラクタ
		 */
		BackgroundSubtractorDynamicMixtureOfGaussian::BackgroundSubtractorDynamicMixtureOfGaussian(double threshold,double alpha,double sigma):width(0),height(0),sigma(sigma),threshold(threshold),alpha(alpha){
			doUpdateInApply(true);
		}

		/*!
		 * @brief デストラクタ
		 */
		BackgroundSubtractorDynamicMixtureOfGaussian::~BackgroundSubtractorDynamicMixtureOfGaussian(){

		}
		/*!
		 * @brief apply関数の中で背景モデルの更新を行うか、あるいは後で別個に行うかを決める
		 * */
		void BackgroundSubtractorDynamicMixtureOfGaussian::doUpdateInApply(bool update){
			updateInApply = update;
			if(!update){
				probsVec.clear();
			}
		}

		/*!
		 * @brief 背景画像を得る。
		 * */
		void BackgroundSubtractorDynamicMixtureOfGaussian::apply(const Image& img,BGSubtractedImage* oimg){
			if(img.getHeight()!=(int)height||img.getWidth()!=(int)width){
				cerr << "Message: There are no BGModels. We first create it.";
				setBGImage(img,sigma);
			}

			if(img.getWidth()!=oimg->getWidth()){
				oimg->setWidth(img.getWidth());
			}
			if(img.getHeight()!=oimg->getHeight()){
				oimg->setHeight(img.getHeight());
			}
			if(img.getChannels()!=oimg->getChannels()){
				oimg->setChannels(img.getChannels());
			}
			if(img.getDepth()!=oimg->getDepth()){
				oimg->setDepth(img.getDepth());
			}


			MmplVector *probs = NULL;
			if(updateInApply){
				probs = new MmplVector();
			}
			if(probsVec.size()!=bgmodel.size()){
				probsVec.clear();
				probsVec.resize(bgmodel.size());
			}


			size_t i=0;
			MmplVector colvec;
			colvec.resize(3,0.0);
			Color col;
			int maxidx;
			for(int y=0;y<height;y++){
				for(int x=0;x<width;x++,i++){
					col = img.getColor(x,y);
					colvec[0] = col[0];
					colvec[1] = col[1];
					colvec[2] = col[2];

					//! マッチング結果を使って後で背景モデルの更新を行う場合
					if(!updateInApply){
						probs = &probsVec[i];
					}

					calcProb(colvec,bgmodel[i],probs);
					maxidx = probs->argmax();
					if(probs->at(maxidx)>threshold){
						//! 背景とマッチしたことになるので背景として処理
						oimg->setColor(x,y,oimg->getBGColor());

						if(updateInApply){
							//! 入力画像を背景モデルに反映させる
							updateByBackGroundColor(&bgmodel[i],col,*probs);
						}
					}
					else{
						//! 背景とはマッチしなかったことになるので、前景として処理
						oimg->setColor(x,y,col);
						if(updateInApply){
							//! 入力画像を背景モデルに反映させる
							updateByForeGroundColor(&bgmodel[i],col,*probs);
						}
					}
				}
			}

			if(updateInApply){
				delete probs;
			}
		}



		/*!
		 * @brief 分布とのマッチング結果と前景と判定された色を利用して背景モデルを更新する
		 * @param mog 更新する混合ガウス分布
		 * @param col 更新する画像の前景と判定された画素値
		 * @param probs 現状の混合ガウス分布とのマッチング結果
		 */
		void BackgroundSubtractorDynamicMixtureOfGaussian::updateByForeGroundColor(MixtureOfSphericalGaussian* mog, const Color& col,const MmplVector& probs){
			int minMachingScoreIdx = 0;
			double minMachingScore = DBL_MAX;
			MmplVector matchingScore = mog->weights;
			int c;
			for(unsigned int i=0;i<mog->weights.size();i++){
				matchingScore[i] /= mog->distributions[i].sigma;
				if(minMachingScore > matchingScore[i]){
					minMachingScore = matchingScore[i];
					minMachingScoreIdx = i;
				}
			}
			//! 最も照合度(matchingScore)が低い分布の平均値を現在の色に置き換える
			for(c=0;c<3;c++){
				mog->distributions[minMachingScoreIdx].means[c] = col[c];
			}
		}

		/*!
		 * @brief 分布とのマッチング結果と背景と判定された色を利用して背景モデルを更新する
		 * @param mog 更新する混合ガウス分布
		 * @param col 更新する画像の背景と判定された画素値
		 * @param probs 現状の混合ガウス分布とのマッチング結果
		 */
		void BackgroundSubtractorDynamicMixtureOfGaussian::updateByBackGroundColor(MixtureOfSphericalGaussian* mog, const Color& col,const MmplVector& probs){
			double rho, r_rho; // 学習率、alphaとprob[i]から計算したものをこれに格納
			double dsigma; // 分散の変化量を一時的に格納
			int c;

			unsigned int MaxDist = probs.argmax();
			for(unsigned int i=0;i<mog->weights.size();i++){
				mog->weights[i] *= 1-alpha;
				if(i == MaxDist && probs[i] > threshold){
					//! マッチした分布についてはalphaを足す
					mog->weights[i] += alpha;
					//! マッチした分布については平均と分散を更新
					rho = alpha * probs[i];
					r_rho = 1.0-rho;
					dsigma = 0;
					for(c=0;c<3;c++){
						mog->distributions[i].means[c] *= r_rho;
						mog->distributions[i].means[c] += rho*col[c];
						dsigma += pow(mog->distributions[i].means[c]-col[c],2);
					}
					mog->distributions[i].sigma *= r_rho;
					mog->distributions[i].sigma += rho*dsigma;
				}
			}
			mog->weights.regularize();
		}

		/*!
		 * @brief マスクで指定された領域以外の背景モデルを更新する
		 * @param img 更新に用いる画像(背景差分前の画像)
		 * @param probsVec 各画素とモデルのマッチング結果が既に格納された配列
		 * @param regionLabels width*heightの長さの領域ラベル配列(不正な長さだとアクセス違反の可能性)
		 * @param maskRegionFlags regionLabelsのうち、flagがtrueになっている領域のみ更新作業から除外する
		 * */
		void BackgroundSubtractorDynamicMixtureOfGaussian::update(const Image& img,const short int* regionLabels, const std::vector<bool>& maskRegionFlags){
			if(probsVec.size()!=bgmodel.size()){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: invalid probsVec has input. No update has done." << endl;
				return;
			}
			if(updateInApply){
					std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: duplicate updating process? The BGModel may be updated in apply function. Use member function 'doUpdateInApply(bool)'." << endl;
			}
			size_t i=0;

			for(int y=0;y<height;y++){
				for(int x=0;x<width;x++,i++,regionLabels++){
					//! マスクされた領域以外の画素をupdateする
					if( probsVec[i].max() > threshold){
						//! 背景処理
						updateByBackGroundColor(&bgmodel[i],img.getColor(x,y),probsVec[i]);
					}
					else if((*regionLabels)==0 || !maskRegionFlags[(*regionLabels)-1]){
						//! 前景でマスクされていない領域の処理
						updateByForeGroundColor(&bgmodel[i],img.getColor(x,y),probsVec[i]);
					}
				}
			}
		}

		/*!
		 * @brief 各画素に対応する背景モデル(混合正規分布)の分布数を輝度値として画像を作って返す
		 * @param oimg 出力先の画像. 分布数が多くて輝度値がオーバーフローした画素は赤くなる
		 * @param amplifier 分布数の差1に対する輝度値の差. デフォルト値は32(分布数8で256を越えてオーバーフローとなる) 
		 * */
		void BackgroundSubtractorDynamicMixtureOfGaussian::getBGDistNumImage(Image* oimg, double amplifier)const{
			if(width==0||height==0){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: this instance has no bgmodels." << endl;
				return;
			}
			else if(oimg->getWidth()!=(int)width || oimg->getHeight()!= (int)height){
				oimg->setWidth(width);
				oimg->setHeight(height);
				oimg->setChannels(3);
			}

			if(amplifier < 0 || amplifier > 255){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "invalid value '" << amplifier << "' is input as parameter 'amplifier'." << endl;
				return;
			}

			Color col;
			int num;
			int i = 0;
			for(int y=0;y<height;y++){
				for(int x=0;x<width;x++,i++){
					num = static_cast<int>(amplifier * bgmodel[i].distributions.size());
					if(num>255){
						col[0] = 0;
						col[1] = 0;
						col[2] = 255;
					}
					else{
						col[0] = (unsigned char)num;
						col[1] = col[0];
						col[2] = col[0];
					}
					oimg->setColor(x,y,col);
				}
			}
		}

		/*!
		 * @brief 指定した順位の重みをもつ正規分布の平均値からなる画像を作成する
		 */
		void BackgroundSubtractorDynamicMixtureOfGaussian::getBGMeanImage(int rank,Image* oimg)const{
			if(width==0||height==0){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: this instance has no bgmodels." << endl;
				return;
			}
			else if(oimg->getWidth()!=(int)width || oimg->getHeight()!= (int)height){
				oimg->setWidth(width);
				oimg->setHeight(height);
				oimg->setChannels(3);
			}
			if(rank<=0){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: invalid value '" << rank << "' is input as parameter 'rank'." << endl;
				return;
			}
			Color col;
			size_t i = 0;
			for(int y=0;y<height;y++){
				for(int x=0;x<width;x++,i++){
					int index = bgmodel[i].weights.argLarge(rank);
					if(index<0){
						col = Color(0,0,0);
					}
					else{
						col[0] = static_cast<unsigned char>(bgmodel[i].distributions.at(index).means.at(0));
						col[1] = static_cast<unsigned char>(bgmodel[i].distributions.at(index).means.at(1));
						col[2] = static_cast<unsigned char>(bgmodel[i].distributions.at(index).means.at(2));
					}
					oimg->setColor(x,y,col);
				}
			}
		}

		/*!
		 * @brief 背景画像から背景モデルに新たな正規分布を作成する
		 * */
		void BackgroundSubtractorDynamicMixtureOfGaussian::setBGImage(const Image& bgimage, double sigma){
			//! sigma <= 0 ならメンバ変数にセットされているsigmaの値を使う
			if(sigma<=0){sigma = this->sigma;}

			if(width!=bgimage.getWidth() || height!=bgimage.getHeight()){
				if(width!=0||height!=0){
					std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
					cerr<<"Warning: invalid size of bgimage. Old Model are cleared." << endl;
					bgmodel.clear();
				}
				width = bgimage.getWidth();
				height = bgimage.getHeight();
				bgmodel.resize(width*height);
			}
			SphericalGaussian newDist;
			newDist.means.resize(3,0.0);
			newDist.sigma = sigma;
			Color col;
			int i = 0;
			for(int y=0;y<height;y++){
				for(int x=0;x<width;x++,i++){
					col = bgimage.getColor(x,y);
					newDist.means[0] = col[0];
					newDist.means[1] = col[1];
					newDist.means[2] = col[2];
					if(bgmodel[i].distributions.empty()){
						bgmodel[i].distributions.push_back(newDist);
						bgmodel[i].weights.push_back(1.0);
					}
					bgmodel[i].distributions.push_back(newDist);
					bgmodel[i].weights.push_back(bgmodel[i].weights.getMean()/2);
					bgmodel[i].weights.regularize();
				}
			}
		}

		/******* Load/Save *********/

		void BackgroundSubtractorDynamicMixtureOfGaussian::load(const string& filename){
			CSVReader csvr(filename);
			vector<string> col = csvr.getCol();
			if(col.size()<4){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: too few parameters. Skip to set parameters" << endl;
			}
			else{
				width = atoi(col[0].c_str());
				height = atoi(col[1].c_str());	
				threshold = atof(col[2].c_str());
				alpha = atof(col[3].c_str());
			}
			size_t pixelNum = width * height;
			bgmodel.clear();
			bgmodel.resize(pixelNum);
			for(size_t i=0;i<pixelNum;i++){
				readFromCsv(&bgmodel[i],csvr);
			}

		}
		void BackgroundSubtractorDynamicMixtureOfGaussian::save(const string& filename)const{
			ofstream fout;
			fout.open(filename.c_str());
			if(!fout){
				std::cerr << "at " << __FILE__ << ": " << __LINE__ << std::endl;
				cerr << "Warning: failed to open the file '" << filename << "'." << endl;
				return;
			}

			fout << width << ",";
			fout << height << ",";
			fout << threshold << ",";
			fout << alpha << endl;
			for(size_t i=0;i<bgmodel.size();i++){
				writeAsCsv(bgmodel[i],fout);
			}
		}

		/******* Accessor *********/

		size_t BackgroundSubtractorDynamicMixtureOfGaussian::getWidth()const{
			return width;
		}

		size_t BackgroundSubtractorDynamicMixtureOfGaussian::getHeight()const{
			return height;
		}

		void BackgroundSubtractorDynamicMixtureOfGaussian::setBGModel(const vector<MixtureOfSphericalGaussian>& bgmodel){
			this->bgmodel = bgmodel;
		}

		const vector<MixtureOfSphericalGaussian>* BackgroundSubtractorDynamicMixtureOfGaussian::getBGModel()const{
			return &bgmodel;
		}

		void BackgroundSubtractorDynamicMixtureOfGaussian::setSigma(double sigma){
			if(sigma<=0){
				cerr << "Error: invalid value of sigma (= " << sigma << ")." << endl;
				return;
			}
			this->sigma = sigma;
		}

		void BackgroundSubtractorDynamicMixtureOfGaussian::setThreshold(double threshold){
			this->threshold = threshold;
		}

		double BackgroundSubtractorDynamicMixtureOfGaussian::getThreshold()const{
			return threshold;
		}

		void BackgroundSubtractorDynamicMixtureOfGaussian::setAlpha(double alpha){
			this->alpha = alpha;
		}

		double BackgroundSubtractorDynamicMixtureOfGaussian::getAlpha()const{
			return alpha;
		}
	}
}
